import sensor, image, time
from pyb import UART, LED
from image import SEARCH_EX

def digit_count(number):
    return len(str(abs(number)))
def coorduart(dire):
    x_part = '0' * (2 - digit_count(dire)) + str(dire)
    return "#DIR{x_part}!".format(x_part=x_part)

sensor.reset()
sensor.set_framesize(sensor.QQVGA)
sensor.set_pixformat(sensor.RGB565)
sensor.skip_frames(time=2000)
sensor.set_hmirror(True)
sensor.set_vflip(True)

LED(1).on()
LED(2).on()
LED(3).on()

clock = time.clock()
uart = UART(3, 115200)
uart.init(115200, bits=8, parity=None, stop=1)

dectection_Flag = True
match_Flag = False
targetNum = None
posResult = 0

template_dict = {}
for i in [1,2,3,4,5,6,7,8]:
    template_dict[i] = {
        #'target': image.Image("./target_template/{}.pgm".format(i)),
        'target2': image.Image("./target_template/{}_2.pgm".format(i)),
        'left': image.Image("./model_temp/{}_l1.pgm".format(i)),
        'left2': image.Image("./model_temp/{}_l2.pgm".format(i)),
        'right': image.Image("./model_temp/{}_r1.pgm".format(i)),
        'right2': image.Image("./model_temp/{}_r2.pgm".format(i))
    }

def templateMatch(gray_img, dectection_Flag, imgPath):
    global posResult
    numRoi = [2, 10, 155, 60]
    if dectection_Flag:
        #template = template_dict[imgPath]['target']
        template2 = template_dict[imgPath]['target2']
        r = gray_img.find_template(template2, 0.80, roi=[20, 5, 120, 110], step=1, search=SEARCH_EX) #or gray_img.find_template(template, 0.80, roi=[20, 5, 120, 110], step=1, search=SEARCH_EX)
    else:
        template = template_dict[imgPath]['left']
        template2 = template_dict[imgPath]['left2']
        r = gray_img.find_template(template, 0.80, roi=numRoi, step=1, search=SEARCH_EX) or gray_img.find_template(template2, 0.80, roi=numRoi, step=1, search=SEARCH_EX)
        posResult=1;
        if not r:
            template = template_dict[imgPath]['right']
            template2 = template_dict[imgPath]['right2']
            r = gray_img.find_template(template, 0.80, roi=numRoi, step=1, search=SEARCH_EX) or gray_img.find_template(template2, 0.80, roi=numRoi, step=1, search=SEARCH_EX)
            posResult=2;
    return r

while(True):
    uart.write("#HELLO!")
    time.sleep_ms(1)
    if(uart.read()==b'R'):
        LED(3).off()
        break

while(True):
    clock.tick()
    img = sensor.snapshot()
    gray_img = img.to_grayscale()

    if dectection_Flag:
        if targetNum == None:
            for i in [1,2,3,4,5,6,7,8]:
               if templateMatch(gray_img, True, i):
                   targetNum = i
                   LED(2).off()
                   break
        else:
            room="#ROOM%d!" %targetNum
            uart.write(room)
            time.sleep_ms(1)
            if(uart.read()==b'G'):
                LED(1).off()
                dectection_Flag = False
                match_Flag = True


    if match_Flag:
        matchResult = templateMatch(gray_img, False, targetNum)
        if matchResult:
            print("检测到病房号:", targetNum)
            print("目标：",matchResult)
            if posResult:
                print("方向：",posResult)
                uart.write(coorduart(posResult))
        posResult = 0

